#include "../include/SerialPort.hpp"

namespace wmj
{
    void showPositionFunc()
    {
        while (1)
        {
            cv::imshow("position", showPosition);
            if (cv::waitKey(20) == 'p')
            {
                return;
            }
        }
    }
    SerialPort::SerialPort()
    {
        getInfo();
        checkPort();
        m_temp_buffer.resize(34, 0x00);
        m_buffer.reserve(512);
        m_read_gimbal_buffer.resize(8);
        m_read_maincontrl_buffer.resize(8);
        m_read_chassis_buffer.resize(8);
        m_read_gyro_buffer.resize(8);
        m_read_shooter_buffer.resize(8);
        m_read_judge_buffer.resize(8);
        m_read_communicate_buffer.resize(8);
        m_read_gyro_angle_buffer.resize(8);
        m_read_gyro_angular_velocity_buffer.resize(8);
        m_read_gyro_acce_buffer.resize(8);
        this->startThread();
        showPosition = cv::Mat(cv::Size(1280, 600), CV_8SC3);
        firstPosition = cv::Point2d(0.0, 0.0);
        std::thread(showPositionFunc)
            .detach();
    }
    void SerialPort::startThread()
    {
        std::cout << "set serial param.\n";
        m_sp_input_service = boost::make_shared<boost::asio::io_service>();
        m_sp_output_service = boost::make_shared<boost::asio::io_service>();

        m_sp_recv_port = boost::make_shared<boost::asio::serial_port>(*m_sp_input_service, m_read_device);
        m_sp_send_port = boost::make_shared<boost::asio::serial_port>(*m_sp_output_service, m_write_device);

        m_sp_recv_port->set_option(boost::asio::serial_port::baud_rate(m_baud_read));                                     // 特率
        m_sp_recv_port->set_option(boost::asio::serial_port::flow_control(boost::asio::serial_port::flow_control::none)); // 流量控制
        m_sp_recv_port->set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));             // 奇偶校验
        m_sp_recv_port->set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one));        // 停止位
        m_sp_recv_port->set_option(boost::asio::serial_port::character_size(8));                                          // 数据位

        m_sp_send_port->set_option(boost::asio::serial_port::baud_rate(m_baud_write));                                    // 特率
        m_sp_send_port->set_option(boost::asio::serial_port::flow_control(boost::asio::serial_port::flow_control::none)); // 流量控制
        m_sp_send_port->set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));             // 奇偶校验
        m_sp_send_port->set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one));        // 停止位
        m_sp_send_port->set_option(boost::asio::serial_port::character_size(8));                                          // 数据位
        // this->initCanMode();
        m_sp_read_t = boost::make_shared<boost::thread>([&]()
                                                        {
            startRead();
            boost::asio::io_service::work work(*m_sp_input_service);
            m_sp_input_service->run(); });
        m_sp_write_t = boost::make_shared<boost::thread>([&]()
                                                         {
            startWrite();
            boost::asio::io_service::work work(*m_sp_output_service);
            m_sp_output_service->run(); });
        std::cout << _warning("thread started.") + "\n";
    }

    SerialPort::~SerialPort()
    {
        stopThread();
    }

    void SerialPort::startRead()
    {
        boost::mutex::scoped_lock lock(m_serial_recv_mutex);
        m_sp_recv_port->async_read_some(boost::asio::buffer(m_temp_buffer, 256),
                                        [&](boost::system::error_code ec, std::size_t bytes_transferred)
                                        {
                                            boost::mutex::scoped_lock lock(m_buffer_decode_mutex);
                                            m_buffer.clear();
                                            for (int i = 0; i < bytes_transferred; ++i)
                                            {

                                                m_buffer.push_back(m_temp_buffer[i]);
                                            }
                                            if (m_debug)
                                            {
                                                std::cout
                                                    << "debug read buffer is ";
                                                for (auto i : m_buffer)
                                                    std::cout << std::hex << (int)i << " ";
                                                m_buffer.clear();
                                                std::cout << "\n";
                                            }
                                            decode(m_buffer);
                                            this->startRead();
                                        });
    }

    void SerialPort::startWrite()
    {
        boost::mutex::scoped_lock lock(m_serial_send_mutex);
        if (m_send_data_seq.empty() == false)
        {
            // if(true)
            // {
            // std::cout << "debug write frame is ";
            // wmj::Buffer temp = m_send_data_seq.front();
            // for(int i = 0; i < temp.size(); i++)
            //     std::cout << std::hex << temp[i] << " ";
            // std::cout << std::endl;
            // }
            // m_sp_recv_port->write_some(boost::asio::buffer(m_send_data_seq.front()));
            boost::asio::async_write(*m_sp_send_port, boost::asio::buffer(m_send_data_seq.front(), m_send_data_seq.front().size()),
                                     [&](boost::system::error_code ec, std::size_t bytes_transferred)
                                     {
                                         boost::mutex::scoped_lock lock(m_buffer_send_mutex);
                                         if (m_send_data_seq.empty() == false)
                                         {
                                             m_send_data_seq.pop();
                                             startWrite();
                                         }
                                     });
        }
    }

    /**
     *@brief: 根据格式编码
     *
     *@param: Buffer& 要传输的数据 Buffer& 编好码的数据
     */
    void SerialPort::encode(Buffer &data, Buffer &frame)
    {
    }

    void SerialPort::decode(wmj::Buffer data)
    {
        wmj::Buffer temp = m_buffer;
        if (temp == std::vector<uint8_t>{0x01, 0x02, 0x6b})
        {
            std::cout << "返回操作成功" << std::endl;
        }
        if (temp == std::vector<uint8_t>{0x01, 0xEE, 0x6b})
        {
            std::cout << "返回操作失败" << std::endl;
        }
        if (temp.size() == 6)
        {
            std::cout << "返回实时位置";
            int32_t position = (int32_t)(((int32_t)temp[1] << 24) | ((int32_t)temp[2] << 16) | ((int32_t)temp[3] << 8) | ((int32_t)temp[4] << 0));
            drawPosition(position);
            std::cout << position << std::endl;
        }
    }
    void SerialPort::drawPosition(int position)
    {
        if (firstPosition.x == 0.0)
        {
            firstPosition = cv::Point2d(wmj::now(), position);
        }
        cv::Point2d tempCircle = cv::Point2d((wmj::now() - firstPosition.x) * 100, (position - firstPosition.y) / 100 + 300);
        if (tempCircle.x > showPosition.cols)
        {
            showPosition = cv::Mat(cv::Size(1280, 600), CV_8SC3);
            firstPosition = cv::Point2d(wmj::now(), position);
        }
        cv::circle(showPosition, tempCircle, 1, cv::Scalar(255, 255, 255));
    }
    void SerialPort::decodeFrame(Buffer frame)
    {
    }

    /**
     * @brief Judge回传清空data1和data2读取实时弹速
     */
    Buffer SerialPort::readFrame(int flag)
    {
    }

    bool SerialPort::sendFrame(Buffer &data)
    {
        boost::mutex::scoped_lock lock(m_buffer_send_mutex);
        Buffer frame = data;
        // encode(data, frame);
        // std::cout<<m_send_data_seq.size()<<std::endl;
        try
        {
            m_send_data_seq.push(frame);
        }
        catch (std::exception &ex)
        {
            HINT return false;
        }
        // std::cout<<m_send_data_seq.size()<<std::endl;
        this->startWrite();
        return true;
    }

    void SerialPort::initCanMode()
    {
        Buffer frame;
        frame.resize(7);
        frame[0] = 0x01;
        frame[1] = 0xF3;
        frame[2] = 0x01;
        frame[3] = 0x6B;
        frame[4] = 0x00;
        frame[5] = 0x00;
        frame[6] = 0x00;
        m_send_data_seq.push(frame);
        m_sp_recv_port->write_some(boost::asio::buffer(frame));
    }

    void SerialPort::stopThread()
    {
        m_sp_input_service->stop();
        m_sp_output_service->stop();
    }

    void SerialPort::getInfo()
    {
        std::cout << "getinfo" << std::endl;
        cv::FileStorage fs(PORT_CFG, cv::FileStorage::READ);
        fs["SerialPort"]["read"] >> m_read_device;
        fs["SerialPort"]["write"] >> m_write_device;
        fs["SerialPort"]["debug"] >> m_debug;
        fs["SerialPort"]["baud_read"] >> m_baud_read;
        fs["SerialPort"]["baud_write"] >> m_baud_write;
        std::cout << "m_read_device:\t" << m_read_device << std::endl;
        std::cout << "m_write_device:\t" << m_write_device << std::endl;
        std::cout << "m_debug:\t" << m_debug << std::endl;
        std::cout << "baud_read:\t" << m_baud_read << std::endl;
        std::cout << "baud_write:\t" << m_baud_write << std::endl;
        fs.release();
    }

    void SerialPort::checkPort()
    {
        int fd = open(m_read_device.c_str(), O_EXCL, NULL);
        bool ret = false;
        if (fd < 0)
        {
            std::cout << _warning("读串口 [") + _warning(m_read_device) + _warning("] 它无了!!!!!!!!!!!!!!!!!!!") << "\n";
            ret = true;
        }
        close(fd);

        fd = open(m_write_device.c_str(), O_EXCL, NULL);
        if (fd < 0)
        {
            std::cout << _warning("写串口 [") + _warning(m_write_device) + _warning("] 它无了!!!!!!!!!!!!!!!!!!!") << "\n";
            ret = true;
        }
        close(fd);
        if (ret)
        {
            std::cout << _warning("当前存在的串口 : ") << "\n";
            system("ls -l /dev/ttyUSB*");
            exit(0);
        }
        std::cout << "读串口 : " << _yellow(m_read_device) << "\n";
        std::cout << "写串口 : " << _yellow(m_write_device) << "\n";
    }
}
